//---------------------------UDP通讯协议
#include "ocu.h"
#include <rpcWiFi.h>
#include <WiFiUdp.h>

int usb_send_cnt = 0;
char SendBuff_USB[500];
char RxBuffer_USB[255];
char RxStateUSB = 0;
int _data_lenUSB = 0;
int _data_cntUSB = 0;

static float floatFromData(char *data, int *anal_cnt) {
  int i = 0x00;
  i |= (*(data + *anal_cnt + 3) << 24);
  i |= (*(data + *anal_cnt + 2) << 16);
  i |= (*(data + *anal_cnt + 1) << 8);
  i |= (*(data + *anal_cnt + 0));

  *anal_cnt += 4;
  return *(float *)&i;
}

static char charFromData(char *data, int *anal_cnt) {
  int temp = *anal_cnt;
  *anal_cnt += 1;
  return *(data + temp);
}

static int intFromData(char *data, int *anal_cnt) {
  int i = 0x00;
  i |= (*(data + *anal_cnt + 3) << 24);
  i |= (*(data + *anal_cnt + 2) << 16);
  i |= (*(data + *anal_cnt + 1) << 8);
  i |= (*(data + *anal_cnt + 0));
  *anal_cnt += 4;
  return i;
}

static void setDataInt(int i) {
  SendBuff_USB[usb_send_cnt++] = ((i << 24) >> 24);
  SendBuff_USB[usb_send_cnt++] = ((i << 16) >> 24);
  SendBuff_USB[usb_send_cnt++] = ((i << 8) >> 24);
  SendBuff_USB[usb_send_cnt++] = (i >> 24);
}

static void setDataFloat(float f) {
  int i = *(int *)&f;
  SendBuff_USB[usb_send_cnt++] = ((i << 24) >> 24);
  SendBuff_USB[usb_send_cnt++] = ((i << 16) >> 24);
  SendBuff_USB[usb_send_cnt++] = ((i << 8) >> 24);
  SendBuff_USB[usb_send_cnt++] = (i >> 24);
}

void send_udp1(void) {
  int i;
  unsigned char sum = 0;
  unsigned int _cnt = 0;
  int cnt_reg = 0;

  cnt_reg = usb_send_cnt;
  SendBuff_USB[usb_send_cnt++] = 0xAA;
  SendBuff_USB[usb_send_cnt++] = 0xAF;
  SendBuff_USB[usb_send_cnt++] = 0x21;
  SendBuff_USB[usb_send_cnt++] = 0;

  SendBuff_USB[usb_send_cnt++] = (1);
  SendBuff_USB[usb_send_cnt++] = (nav_rx.cal_imu);  //cal mode
  setDataFloat(nav_rx.att_now[0]);                  //vmc_all.att[PITr]);//setDataFloat(5*sin(timer));//vmc_all.att[PITr]);
  setDataFloat(0);
  setDataFloat(0);
  setDataFloat(0);
  setDataFloat(0);
  setDataFloat(0);
  setDataFloat(0);
  setDataFloat(0);
  setDataFloat(0);
  setDataFloat(0);

  if (nav_rx.cal_imu==1)
    nav_rx.cal_imu = 2;
  else if( nav_rx.cal_imu ==2)
    nav_rx.cal_imu = 0;

  SendBuff_USB[cnt_reg + 3] = (usb_send_cnt - cnt_reg) - 4;
  for (i = cnt_reg; i < usb_send_cnt; i++)
    sum += SendBuff_USB[i];
  SendBuff_USB[usb_send_cnt++] = sum;
}

void send_udp2(void) {//send ocu
  int i;
  unsigned char sum = 0;
  unsigned int _cnt = 0;
  int cnt_reg = 0;

  cnt_reg = usb_send_cnt;
  SendBuff_USB[usb_send_cnt++] = 0xAA;
  SendBuff_USB[usb_send_cnt++] = 0xAF;
  SendBuff_USB[usb_send_cnt++] = 0x32;
  SendBuff_USB[usb_send_cnt++] = 0;

  SendBuff_USB[usb_send_cnt++] = ocu.up_mode;
  SendBuff_USB[usb_send_cnt++] = ocu.record.en_record;
  setDataFloat(ocu.rc_spd_w[0]);
  setDataFloat(ocu.rc_spd_w[1]);
  setDataFloat(ocu.rc_att_w[0]);
  setDataFloat(ocu.rc_att_w[1]);
  setDataFloat(ocu.rate_yaw_w);
  SendBuff_USB[usb_send_cnt++] = ocu.key_st;
  SendBuff_USB[usb_send_cnt++] = ocu.key_back;
  setDataInt(ocu.key_lr);
  setDataInt(ocu.key_ud);
  SendBuff_USB[usb_send_cnt++] = ocu.key_x;
  SendBuff_USB[usb_send_cnt++] = ocu.key_y;
  SendBuff_USB[usb_send_cnt++] = ocu.key_b;
  SendBuff_USB[usb_send_cnt++] = ocu.key_a;
  SendBuff_USB[usb_send_cnt++] = ocu.key_ll;
  SendBuff_USB[usb_send_cnt++] = ocu.key_rr;

  SendBuff_USB[cnt_reg + 3] = (usb_send_cnt - cnt_reg) - 4;
  for (i = cnt_reg; i < usb_send_cnt; i++)
    sum += SendBuff_USB[i];
  SendBuff_USB[usb_send_cnt++] = sum;
}

void Anal_UDP(char *data_buf, char num) {
  static int link_cnt = 0;
  char id;
  char temp_char = 0, need_save = 0;
  char sum = 0;
  char i, j;
  float float_temp = 0;
  int anal_cnt = 4;
  for (i = 0; i < (num - 1); i++)
    sum += *(data_buf + i);
  if (!(sum == *(data_buf + num - 1))) {
    i = 0;
    return;  //判断sum
  }
  if (!(*(data_buf) == 0xBA && *(data_buf + 1) == 0xBF)) {
    i = 0;
    return;  //判断帧头
  }
  if (!robot_connect) {
    robot_connect = 1;
    //Serial.println("Robot::UDP OCU Link"+link_cnt++);
  }

  if (*(data_buf + 2) == 0x04)  //robot state
  {
    robot_loss_cnt = 0;
    nav_rx.att_now[0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.att_now[1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.att_now[2] = floatFromData(data_buf, &anal_cnt);
    nav_rx.datt_now[0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.datt_now[1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.datt_now[2] = floatFromData(data_buf, &anal_cnt);
    nav_rx.cog_pos_n.x = floatFromData(data_buf, &anal_cnt);
    nav_rx.cog_pos_n.y = floatFromData(data_buf, &anal_cnt);
    nav_rx.cog_pos_n.z = floatFromData(data_buf, &anal_cnt);
    nav_rx.cog_spd_n.x = floatFromData(data_buf, &anal_cnt);
    nav_rx.cog_spd_n.y = floatFromData(data_buf, &anal_cnt);
    nav_rx.cog_spd_n.z = floatFromData(data_buf, &anal_cnt);
    nav_rx.acc_n_now[0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.acc_n_now[1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.acc_n_now[2] = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[2].x = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[2].y = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[2].z = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[0].x = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[0].y = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[0].z = floatFromData(data_buf, &anal_cnt);
    float_temp = floatFromData(data_buf, &anal_cnt);
    float_temp = floatFromData(data_buf, &anal_cnt);
    nav_rx.dcom_n_tar[0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.dcom_n_tar[1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.dcom_n_tar[2] = floatFromData(data_buf, &anal_cnt);
    nav_rx.com_n_tar[2] = floatFromData(data_buf, &anal_cnt);
    nav_rx.robot_mode   = charFromData(data_buf, &anal_cnt);
  } else if (*(data_buf + 2) == 0x41)  //robot state2
  {
    robot_loss_cnt = 0;
    nav_rx.epos_n_now[3].x = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[3].y = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[3].z = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[1].x = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[1].y = floatFromData(data_buf, &anal_cnt);
    nav_rx.epos_n_now[1].z = floatFromData(data_buf, &anal_cnt);

    char id = 2;
    nav_rx.sita_now[id][0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita_now[id][1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita_now[id][2] = floatFromData(data_buf, &anal_cnt);
    id = 0;
    nav_rx.sita_now[id][0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita_now[id][1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita_now[id][2] = floatFromData(data_buf, &anal_cnt);
    id = 3;
    nav_rx.sita_now[id][0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita_now[id][1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita_now[id][2] = floatFromData(data_buf, &anal_cnt);
    id = 1;
    nav_rx.sita_now[id][0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita_now[id][1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita_now[id][2] = floatFromData(data_buf, &anal_cnt);

    nav_rx.sita1_off[2] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita2_off[2] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita3_off[2] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita1_off[0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita2_off[0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita3_off[0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita1_off[3] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita2_off[3] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita3_off[3] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita1_off[1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita2_off[1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.sita3_off[1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.is_ground[2] = charFromData(data_buf, &anal_cnt);
    nav_rx.is_ground[0] = charFromData(data_buf, &anal_cnt);
    nav_rx.is_ground[1] = charFromData(data_buf, &anal_cnt);
    nav_rx.is_ground[3] = charFromData(data_buf, &anal_cnt);
    

  } else if (*(data_buf + 2) == 0x09)  //record
  {
    robot_loss_cnt = 0;
    nav_rx.temp_record[0] = floatFromData(data_buf, &anal_cnt);
    nav_rx.temp_record[1] = floatFromData(data_buf, &anal_cnt);
    nav_rx.temp_record[2] = floatFromData(data_buf, &anal_cnt);
    nav_rx.temp_record[3] = floatFromData(data_buf, &anal_cnt);
    nav_rx.temp_record[4] = floatFromData(data_buf, &anal_cnt);
    nav_rx.temp_record[5] = floatFromData(data_buf, &anal_cnt);
    nav_rx.temp_record[6] = floatFromData(data_buf, &anal_cnt);
    nav_rx.temp_record[7] = floatFromData(data_buf, &anal_cnt);
    nav_rx.temp_record[8] = floatFromData(data_buf, &anal_cnt);
    nav_rx.temp_record[9] = floatFromData(data_buf, &anal_cnt);
  }
}

void UDP_RX_PROCESS(char *Buf, int Len) {
  int i = 0;
  char com_data;
  for (i = 0; i < Len; i++) {
    com_data = Buf[i];
    if (RxStateUSB == 0 && com_data == 0xBA) {
      RxStateUSB = 1;
      RxBuffer_USB[0] = com_data;
    } else if (RxStateUSB == 1 && com_data == 0xBF) {
      RxStateUSB = 2;
      RxBuffer_USB[1] = com_data;
    } else if (RxStateUSB == 2 && com_data > 0 && com_data < 0XF1) {
      RxStateUSB = 3;
      RxBuffer_USB[2] = com_data;
    } else if (RxStateUSB == 3 && com_data < 255) {
      RxStateUSB = 4;
      RxBuffer_USB[3] = com_data;
      _data_lenUSB = com_data;
      _data_cntUSB = 0;
    } else if (RxStateUSB == 4 && _data_lenUSB > 0) {
      _data_lenUSB--;
      RxBuffer_USB[4 + _data_cntUSB++] = com_data;
      if (_data_lenUSB == 0)
        RxStateUSB = 5;
    } else if (RxStateUSB == 5) {
      RxStateUSB = 0;
      RxBuffer_USB[4 + _data_cntUSB] = com_data;
      Anal_UDP(RxBuffer_USB, _data_cntUSB + 5);
      for (i = 0; i < 255; i++)
        RxBuffer_USB[i] = 0;
    } else
      RxStateUSB = 0;
  }
}